/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Willow Garage, Inc. nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: TKruse
*********************************************************************/

#include <base_local_planner/simple_trajectory_generator.h>

#include <cmath>

#include <base_local_planner/velocity_iterator.h>

namespace base_local_planner
{

void SimpleTrajectoryGenerator::initialise(
        const Eigen::Vector3f &pos,
        const Eigen::Vector3f &vel,
        const Eigen::Vector3f &goal,
        base_local_planner::LocalPlannerLimits *limits,
        const Eigen::Vector3f &vsamples,
        std::vector<Eigen::Vector3f> additional_samples,
        bool discretize_by_time)
{
    initialise(pos, vel, goal, limits, vsamples, discretize_by_time);
    // add static samples if any
    sample_params_.insert(sample_params_.end(), additional_samples.begin(), additional_samples.end());
}

// 实际上，我们在这里生成了所有的速度样本向量，然后从中生成轨迹 discretize_by_time: true则生成的轨迹是以等时间步长，false 则是基于等距离步长
// 通过SimpleTrajectoryGenerator的成员函数initialise()产生一系列的采样空间，之后利用采样空间和样本来得到一系列的轨迹。该文件对这一系列轨迹进行打分，计算得到最优轨迹，并将最优轨迹对应的速度采样下发给底座。
void SimpleTrajectoryGenerator::initialise(
        const Eigen::Vector3f &pos,
        const Eigen::Vector3f &vel,
        const Eigen::Vector3f &goal,
        base_local_planner::LocalPlannerLimits *limits,
        const Eigen::Vector3f &vsamples,
        bool discretize_by_time)
{
    // 获得最大旋转速度
    double max_vel_th = limits->max_vel_theta;
    // 获得最小旋转速度
    double min_vel_th = -1.0 * max_vel_th;
    // 产生轨迹用到; 如果为true,路径基于相同时间步长端被分段,不然基于等距的步长
    discretize_by_time_ = discretize_by_time;
    // 三个加速度
    Eigen::Vector3f acc_lim = limits->getAccLimits();
    pos_ = pos;
    vel_ = vel;
    limits_ = limits;
    next_sample_index_ = 0;
    sample_params_.clear();
    // 获取最小速度和最大速度，从最小线速度到最大线速度产生一系列的采样速度离散点
    double min_vel_x = limits->min_vel_x;
    double max_vel_x = limits->max_vel_x;
    double min_vel_y = limits->min_vel_y;
    double max_vel_y = limits->max_vel_y;
    // x,y方向和角速度的采样个值不能为0
    // if sampling number is zero in any dimension, we don't generate samples generically
    if (vsamples[0] * vsamples[1] * vsamples[2] > 0)
    {
        // compute the feasible velocity space based on the rate at which we run
        Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
        Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();
        // 使用dwa模拟的是一段时间的，否则就是瞬时的
        if (!use_dwa_)
        {
            // 首先用当前位置到目标位置的距离和仿真时间sim_time处理得到max_vel_x和max_vel_y边界，
            // 目的是模拟仿真时间内匀减速到0，刚好到达目标点的情景 sim_time_: 1.7s
            // there is no point in overshooting the goal, and it also may break the robot behavior,
            // so we limit the velocities to those that do not overshoot in sim_time
            double dist = hypot(goal[0] - pos[0], goal[1] - pos[1]);
            max_vel_x = std::max(std::min(max_vel_x, dist / sim_time_), min_vel_x);
            max_vel_y = std::max(std::min(max_vel_y, dist / sim_time_), min_vel_y);

            // 然后基于acc_lim * sim_time得到一种边界，还有设置的速度参数限制作为一种边界，
            // 选取边界中空间较小的边界。这种策略，能够获得较大的采样空间（因为用了sim_time）?
            // if we use continous acceleration, we can sample the max velocity we can reach in sim_time_
            max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_time_);
            max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_time_);
            max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_time_);

            min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_time_);
            min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_time_);
            min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_time_);
        }
        else
        {
            // DWA方法，直接用acc_lim * sim_period得到边界，还有设置的速度参数限制作为边界，然后选取两种边界中空间较小的边界
            // with dwa do not accelerate beyond the first step, we only sample within velocities we reach in sim_period
            // sim_period_: 0.5
            max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_period_);
            max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_period_);
            max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_period_);

            min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_period_);
            min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_period_);
            min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_period_);
        }
        Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();
        VelocityIterator x_it(min_vel[0], max_vel[0], vsamples[0]);
        VelocityIterator y_it(min_vel[1], max_vel[1], vsamples[1]);
        VelocityIterator th_it(min_vel[2], max_vel[2], vsamples[2]);
        // 得到速度空间边界后，根据x，y，theta三个采样个数进行插补，进而组合出整个速度采样空间
        for (; !x_it.isFinished(); x_it++)
        {
            vel_samp[0] = x_it.getVelocity();
            for (; !y_it.isFinished(); y_it++)
            {
                vel_samp[1] = y_it.getVelocity();
                for (; !th_it.isFinished(); th_it++)
                {
                    vel_samp[2] = th_it.getVelocity();
                    // ROS_DEBUG("Sample %f, %f, %f", vel_samp[0], vel_samp[1], vel_samp[2]);
                    // 存放采样空间的组合
                    sample_params_.push_back(vel_samp);
                }
                th_it.reset();
            }
            y_it.reset();
        }
    }
}

void SimpleTrajectoryGenerator::setParameters(
        double sim_time, double sim_granularity, double angular_sim_granularity, bool use_dwa, double sim_period)
{
    sim_time_ = sim_time;
    sim_granularity_ = sim_granularity;
    angular_sim_granularity_ = angular_sim_granularity;
    use_dwa_ = use_dwa;
    continued_acceleration_ = !use_dwa_;
    sim_period_ = sim_period;
}

/** 产生器是否可以生成多个局部路径
* Whether this generator can create more trajectories
*/
bool SimpleTrajectoryGenerator::hasMoreTrajectories()
{
    return next_sample_index_ < sample_params_.size();
}

/** 产生和返回下个采样局部路径
* Create and return the next sample trajectory
*/
bool SimpleTrajectoryGenerator::nextTrajectory(Trajectory &comp_traj)
{
    bool result = false;
    if (generateTrajectory(pos_, vel_, sample_params_[next_sample_index_], comp_traj)) result = true;
    next_sample_index_++;
    return result;
}

/**
* @param pos current position of robot 机器人当前位姿
* @param vel desired velocity for sampling 期望采样的速度
*/
bool SimpleTrajectoryGenerator::generateTrajectory(
        Eigen::Vector3f pos,
        Eigen::Vector3f vel,
        Eigen::Vector3f sample_target_vel,
        base_local_planner::Trajectory &traj)
{
    double vmag = hypot(sample_target_vel[0], sample_target_vel[1]);
    double eps = 1e-4;
    traj.cost_ = -1.0; // placed here in case we return early
    //trajectory might be reused so we'll make sure to reset it
    traj.resetPoints();
    // make sure that the robot would at least be moving with one of
    // the required minimum velocities for translation and rotation (if set)
    // 先判断速度是否满足下面调节的其中一个
    // 1. 平移速度不小于min_trans_vel且旋转速度不小于min_rot_vel
    // 2. 平移速度不大于max_trans_vel
    // 如果满足，则直接返回false，此时，traj为空路径
    if ((limits_->min_vel_trans >= 0 && vmag + eps < limits_->min_vel_trans) &&
        (limits_->min_vel_theta >= 0 && fabs(sample_target_vel[2]) + eps < limits_->min_vel_theta)) return false;
    // 如果在x和y方向的速度向量之和的大小超过了速度限制，返回false
    // make sure we do not exceed max diagonal (x+y) translational velocity (if set)
    if (limits_->max_vel_trans >= 0 && vmag - eps > limits_->max_vel_trans) return false;
    // 确定仿真步数
    int num_steps;
    // 生成的轨迹是基于等时间步长
    if (discretize_by_time_) num_steps = ceil(sim_time_ / sim_granularity_);
    else
    {
        // 生成的轨迹是基于等距离步长， 计算步数num_steps
        //compute the number of steps we must take along this trajectory to be "safe"
        double sim_time_distance = vmag * sim_time_;
        // the distance the robot would travel in sim_time if it did not change velocity 由当前速度推出在sim_time_内走过的距离
        // the angle the robot would rotate in sim_time 由当前速度推出在sim_time_内转过的角度
        double sim_time_angle = fabs(sample_target_vel[2]) * sim_time_;
        num_steps = ceil(std::max(sim_time_distance / sim_granularity_,
                              sim_time_angle / angular_sim_granularity_));
    }
    // 步数不能为零
    if (num_steps == 0) return false;
    // compute a timestep 确定仿真步长
    double dt = sim_time_ / num_steps;
    traj.time_delta_ = dt;
    // 对loop_vel进行初始化
    Eigen::Vector3f loop_vel;
    if (continued_acceleration_)
    {
        // use_dwa == false，则采用连续加速的策略
        // 即仿真出的轨迹中不同点对应的速度是变化的，此时将轨迹中保存的对应速度设为基于当前速度第一次加速出的速度。
        // 否则，轨迹中的各个点为同样的速度，即sample_target_vel，此时轨迹中保存的速度也是该速度
        // assuming the velocity of the first cycle is the one we want to store in the trajectory object
        loop_vel = computeNewVelocities(sample_target_vel, vel, limits_->getAccLimits(), dt);
        traj.xv_ = loop_vel[0];
        traj.yv_ = loop_vel[1];
        traj.thetav_ = loop_vel[2];
    }
    else
    {
        // 如果use_dwa==true，用恒速计算轨迹点位姿
        // assuming sample_vel is our target velocity within acc limits for one timestep
        loop_vel = sample_target_vel;
        traj.xv_ = sample_target_vel[0];
        traj.yv_ = sample_target_vel[1];
        traj.thetav_ = sample_target_vel[2];
    }

    // 推算局部路径，为了之后碰撞检测和更新路径代价
    // simulate the trajectory and check for collisions, updating costs along the way
    for (int i = 0; i < num_steps; ++i)
    {
        // add the point to the trajectory so we can draw it later if we want 把单个路径点放到局部路径traj中
        traj.addPoint(pos[0], pos[1], pos[2]); // 对轨迹的位置点的位姿进行添加
        if (continued_acceleration_)
        {
            // calculate velocities 计算速度
            loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
            // ROS_WARN_NAMED("Generator", "Flag: %d, Loop_Vel %f, %f, %f", continued_acceleration_, loop_vel[0], loop_vel[1], loop_vel[2]);
        }
        // 用传入的速度更新机器人的位置
        // update the position of the robot using the velocities passed in
        pos = computeNewPositions(pos, loop_vel, dt);  // 计算模拟轨迹的下一个点的位姿
    } // end for simulation steps 仿真阶段结束
    return true; // trajectory has at least one point 局部路径至少要包含一个点
}

Eigen::Vector3f SimpleTrajectoryGenerator::computeNewPositions(const Eigen::Vector3f &pos,  // 计算轨迹下一个点的位姿
                                                               const Eigen::Vector3f &vel, double dt)
{
    Eigen::Vector3f new_pos = Eigen::Vector3f::Zero();
    new_pos[0] = pos[0] + (vel[0] * cos(pos[2]) + vel[1] * cos(M_PI_2 + pos[2])) * dt;
    new_pos[1] = pos[1] + (vel[0] * sin(pos[2]) + vel[1] * sin(M_PI_2 + pos[2])) * dt;
    new_pos[2] = pos[2] + vel[2] * dt;
    return new_pos;
}

/**
* change vel using acceleration limits to converge towards sample_target-vel 根据加速度限制改变速度，向采样目标速度收敛
*/
Eigen::Vector3f SimpleTrajectoryGenerator::computeNewVelocities(const Eigen::Vector3f &sample_target_vel,
                                                                const Eigen::Vector3f &vel,
                                                                Eigen::Vector3f acclimits, double dt)
{
    Eigen::Vector3f new_vel = Eigen::Vector3f::Zero();
    for (int i = 0; i < 3; ++i)
    {
        if (vel[i] < sample_target_vel[i]) new_vel[i] = std::min(double(sample_target_vel[i]), vel[i] + acclimits[i] * dt);
        else new_vel[i] = std::max(double(sample_target_vel[i]), vel[i] - acclimits[i] * dt);
    }
    return new_vel;
}

} /* namespace base_local_planner */
